/*
 *   MX28.h
 *
 *   Author: ROBOTIS
 *
 */

#ifndef _MX_28_H_
#define _MX_28_H_

//#define MX28_1024

namespace Robot
{
	class MX28
	{
	public:
		static const int MIN_VALUE;
//#ifdef MX28_1024
//        static const int CENTER_VALUE;
//        static const int MAX_VALUE;
//        static const double MIN_ANGLE;
//        static const double MAX_ANGLE;
//        static const double RATIO_VALUE2ANGLE;
//        static const double RATIO_ANGLE2VALUE;
//
//		static const int ZERO1;
//		static const int ZERO2;
//		static const int ZERO3;
//		static const int ZERO4;
//		static const int ZERO5;
//		static const int ZERO6;
//		static const int ZERO7;
//		static const int ZERO8;
//		static const int ZERO9;
//		static const int ZERO10;
//		static const int ZERO11;
//		static const int ZERO12;
//		static const int ZERO13;
//		static const int ZERO14;
//		static const int ZERO15;
//		static const int ZERO16;
//		static const int ZERO17;
//		static const int ZERO18;
//		static const int ZERO19;
//		static const int ZERO20;
//
//        static const int PARAM_BYTES;
//#else
		static const int CENTER_VALUE;
		static const int MAX_VALUE;
		static const double MIN_ANGLE;
		static const double MAX_ANGLE;
		static const double RATIO_VALUE2ANGLE;
		static const double RATIO_ANGLE2VALUE;

		static const int ZERO1;
		static const int ZERO2;
		static const int ZERO3;
		static const int ZERO4;
		static const int ZERO5;
		static const int ZERO6;
		static const int ZERO7;
		static const int ZERO8;
		static const int ZERO9;
		static const int ZERO10;
		static const int ZERO11;
		static const int ZERO12;
		static const int ZERO13;
		static const int ZERO14;
		static const int ZERO15;
		static const int ZERO16;
		static const int ZERO17;
		static const int ZERO18;
		static const int ZERO19;
		static const int ZERO20;

		static const int PARAM_BYTES;
//#endif

        static int GetMirrorValue(int value)		{ return MAX_VALUE + 1 - value; }
		static double GetMirrorAngle(double angle)	{ return -angle; }

		static int Angle2Value(double angle) {  return (int)(angle*RATIO_ANGLE2VALUE)+CENTER_VALUE; }
		static double Value2Angle(int value) { return (double)(value-CENTER_VALUE)*RATIO_VALUE2ANGLE; }

//		static int Angle2Value(int id, double angle) {
//
//				switch(id){
//				case 1:
//					return (angle/RATIO_VALUE2ANGLE)+ZERO1;
//				case 2:
//					return -1*(angle/RATIO_VALUE2ANGLE)+ZERO2;
//				case 3:
//					return (angle/RATIO_VALUE2ANGLE)+ZERO3;
//				case 4:
//					return -1*(angle/RATIO_VALUE2ANGLE)+ZERO4;
//				case 5:
//					return (angle/RATIO_VALUE2ANGLE)+ZERO5;
//				case 6:
//					return -1*(angle/RATIO_VALUE2ANGLE)+ZERO6;
//				case 7:
//					return (angle/RATIO_VALUE2ANGLE)+ZERO7;
//				case 8:
//					return -1*(angle/RATIO_VALUE2ANGLE)+ZERO8;
//				case 9:
//					return (angle/RATIO_VALUE2ANGLE)+ZERO9;
//				case 10:
//					return -1*(angle/RATIO_VALUE2ANGLE)+ZERO10;
//				case 11:
//					return -1*(angle/RATIO_VALUE2ANGLE)+ZERO11;
//				case 12:
//					return (angle/RATIO_VALUE2ANGLE)+ZERO12;
//				case 13:
//					return -1*(angle/RATIO_VALUE2ANGLE)+ZERO13;
//				case 14:
//					return (angle/RATIO_VALUE2ANGLE)+ZERO14;
//				case 15:
//					return (angle/RATIO_VALUE2ANGLE)+ZERO15;
//				case 16:
//					return (angle/RATIO_VALUE2ANGLE)+ZERO16;
//				case 17:
//					return (angle/RATIO_VALUE2ANGLE)+ZERO17;
//				case 18:
//					return -1*(angle/RATIO_VALUE2ANGLE)+ZERO18;
//				case 19:
//					return -1*(angle/RATIO_VALUE2ANGLE)+ZERO19;
//				case 20:
//					return -1*(angle/RATIO_VALUE2ANGLE)+ZERO20;
//				default:
//					break;
//				}
//				return 0;
//
//			 }
		//static double Value2Angle(int value) { return (double)(value-CENTER_VALUE)*RATIO_VALUE2ANGLE; }
//		static double Value2Angle(int id, int value) {
//
//				switch(id){
//				case 1:
//					return (double)(value-ZERO1)*RATIO_VALUE2ANGLE;
//				case 2:
//					return -1*(double)(value-ZERO2)*RATIO_VALUE2ANGLE;
//				case 3:
//					return (double)(value-ZERO3)*RATIO_VALUE2ANGLE;
//				case 4:
//					return -1*(double)(value-ZERO4)*RATIO_VALUE2ANGLE;
//				case 5:
//					return (double)(value-ZERO5)*RATIO_VALUE2ANGLE;
//				case 6:
//					return -1*(double)(value-ZERO6)*RATIO_VALUE2ANGLE;
//				case 7:
//					return (double)(value-ZERO7)*RATIO_VALUE2ANGLE;
//				case 8:
//					return -1*(double)(value-ZERO8)*RATIO_VALUE2ANGLE;
//				case 9:
//					return (double)(value-ZERO9)*RATIO_VALUE2ANGLE;
//				case 10:
//					return -1*(double)(value-ZERO10)*RATIO_VALUE2ANGLE;
//				case 11:
//					return -1*(double)(value-ZERO11)*RATIO_VALUE2ANGLE;
//				case 12:
//					return (double)(value-ZERO12)*RATIO_VALUE2ANGLE;
//				case 13:
//					return -1*(double)(value-ZERO13)*RATIO_VALUE2ANGLE;
//				case 14:
//					return (double)(value-ZERO14)*RATIO_VALUE2ANGLE;
//				case 15:
//					return (double)(value-ZERO15)*RATIO_VALUE2ANGLE;
//				case 16:
//					return (double)(value-ZERO16)*RATIO_VALUE2ANGLE;
//				case 17:
//					return (double)(value-ZERO17)*RATIO_VALUE2ANGLE;
//				case 18:
//					return -1*(double)(value-ZERO18)*RATIO_VALUE2ANGLE;
//				case 19:
//					return (double)(value-ZERO19)*RATIO_VALUE2ANGLE;
//				case 20:
//					return (double)(value-ZERO20)*RATIO_VALUE2ANGLE;
//				default:
//					break;
//				}
//				return 0;
//
//			 }
		// Address
//#ifdef MX28_1024
//        enum
//        {
//            P_MODEL_NUMBER_L            = 0,
//            P_MODEL_NUMBER_H            = 1,
//            P_VERSION                   = 2,
//            P_ID                        = 3,
//            P_BAUD_RATE                 = 4,
//            P_RETURN_DELAY_TIME         = 5,
//            P_CW_ANGLE_LIMIT_L          = 6,
//            P_CW_ANGLE_LIMIT_H          = 7,
//            P_CCW_ANGLE_LIMIT_L         = 8,
//            P_CCW_ANGLE_LIMIT_H         = 9,
//            P_HIGH_LIMIT_TEMPERATURE    = 11,
//            P_LOW_LIMIT_VOLTAGE         = 12,
//            P_HIGH_LIMIT_VOLTAGE        = 13,
//            P_MAX_TORQUE_L              = 14,
//            P_MAX_TORQUE_H              = 15,
//            P_RETURN_LEVEL              = 16,
//            P_ALARM_LED                 = 17,
//            P_ALARM_SHUTDOWN            = 18,
//            P_TORQUE_ENABLE             = 24,
//            P_LED                       = 25,
//            P_CW_COMPLIANCE_MARGIN      = 26,
//            P_CCW_COMPLIANCE_MARGIN     = 27,
//            P_CW_COMPLIANCE_SLOPE       = 28,
//            P_CCW_COMPLIANCE_SLOPE      = 29,
//            P_GOAL_POSITION_L           = 30,
//            P_GOAL_POSITION_H           = 31,
//            P_MOVING_SPEED_L            = 32,
//            P_MOVING_SPEED_H            = 33,
//            P_TORQUE_LIMIT_L            = 34,
//            P_TORQUE_LIMIT_H            = 35,
//            P_PRESENT_POSITION_L        = 36,
//            P_PRESENT_POSITION_H        = 37,
//            P_PRESENT_SPEED_L           = 38,
//            P_PRESENT_SPEED_H           = 39,
//            P_PRESENT_LOAD_L            = 40,
//            P_PRESENT_LOAD_H            = 41,
//            P_PRESENT_VOLTAGE           = 42,
//            P_PRESENT_TEMPERATURE       = 43,
//            P_REGISTERED_INSTRUCTION    = 44,
//            P_MOVING                    = 46,
//            P_LOCK                      = 47,
//            P_PUNCH_L                   = 48,
//            P_PUNCH_H                   = 49,
//            MAXNUM_ADDRESS
//        };
//#else
		enum
		{
			P_BYTE						=1,
			P_WORD						=2,
			P_MODEL_NUMBER_L			= 0,
			P_MODEL_NUMBER_H			= 1,
			P_VERSION					= 2,
			P_ID						= 3,
			P_BAUD_RATE					= 4,
			P_RETURN_DELAY_TIME			= 5,			
			P_CW_ANGLE_LIMIT_L          = 6,
			P_CW_ANGLE_LIMIT_H          = 7,
			P_CCW_ANGLE_LIMIT_L         = 8,
			P_CCW_ANGLE_LIMIT_H         = 9,
			P_SYSTEM_DATA2              = 10,
			P_HIGH_LIMIT_TEMPERATURE    = 11,
			P_LOW_LIMIT_VOLTAGE         = 12,
			P_HIGH_LIMIT_VOLTAGE        = 13,
			P_MAX_TORQUE_L              = 14,
			P_MAX_TORQUE_H              = 15,
			P_RETURN_LEVEL				= 16,
			P_ALARM_LED                 = 17,
			P_ALARM_SHUTDOWN            = 18,
			P_OPERATING_MODE            = 19,
			P_LOW_CALIBRATION_L         = 20,
			P_LOW_CALIBRATION_H         = 21,
			P_HIGH_CALIBRATION_L        = 22,
			P_HIGH_CALIBRATION_H        = 23,
			P_TORQUE_ENABLE             = 24,
			P_LED                       = 25,
			P_D_GAIN                    = 26,
			P_I_GAIN                    = 27,
			P_P_GAIN                    = 28,
			P_RESERVED                  = 29,
			P_GOAL_POSITION_L           = 30,
			P_GOAL_POSITION_H           = 31,
			P_MOVING_SPEED_L            = 32,
			P_MOVING_SPEED_H            = 33,
			P_TORQUE_LIMIT_L            = 34,
			P_TORQUE_LIMIT_H            = 35,
			P_PRESENT_POSITION_L        = 36,
			P_PRESENT_POSITION_H        = 37,
			P_PRESENT_SPEED_L           = 38,
			P_PRESENT_SPEED_H           = 39,
			P_PRESENT_LOAD_L            = 40,
			P_PRESENT_LOAD_H            = 41,
			P_PRESENT_VOLTAGE           = 42,
			P_PRESENT_TEMPERATURE       = 43,
			P_REGISTERED_INSTRUCTION	= 44,
			P_PAUSE_TIME                = 45,
			P_MOVING					= 46,
			P_LOCK						= 47,
			P_PUNCH_L					= 48,
			P_PUNCH_H					= 49,
			P_RESERVED4                 = 50,
            P_RESERVED5                 = 51,
            P_POT_L                     = 52,
            P_POT_H                     = 53,
            P_PWM_OUT_L                 = 54,
            P_PWM_OUT_H                 = 55,
            P_P_ERROR_L                 = 56,
            P_P_ERROR_H                 = 57,
            P_I_ERROR_L                 = 58,
            P_I_ERROR_H                 = 59,
            P_D_ERROR_L                 = 60,
            P_D_ERROR_H                 = 61,
            P_P_ERROR_OUT_L             = 62,
            P_P_ERROR_OUT_H             = 63,
            P_I_ERROR_OUT_L             = 64,
            P_I_ERROR_OUT_H             = 65,
            P_D_ERROR_OUT_L             = 66,
            P_D_ERROR_OUT_H             = 67,
			MAXNUM_ADDRESS
		};
//#endif
	};
}

#endif
